#ifndef EGO_PLANNER_ADAPTER__TRAJECTORY_TRACKER_HPP_
#define EGO_PLANNER_ADAPTER__TRAJECTORY_TRACKER_HPP_

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>
#include <std_msgs/msg/float64.hpp>
#include <visualization_msgs/msg/marker.hpp>
#include <tf2/LinearMath/Transform.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <Eigen/Dense>
#include <memory>
#include <vector>
#include <mutex>

// Forward declarations for ego-planner messages if available
namespace traj_utils {
namespace msg {
class Bspline;
}
}

namespace ego_planner_adapter {

/**
 * @brief PID Controller for position control
 */
class PIDController {
public:
    PIDController(double kp, double ki, double kd, double max_output, double min_output);
    
    double update(double error, double dt);
    void reset();
    void setGains(double kp, double ki, double kd);

private:
    double kp_, ki_, kd_;
    double max_output_, min_output_;
    double integral_;
    double previous_error_;
    bool first_call_;
};

/**
 * @brief B-spline trajectory interpolator
 */
class BSplineInterpolator {
public:
    BSplineInterpolator();
    
    bool setTrajectory(const std::shared_ptr<const traj_utils::msg::Bspline>& bspline_msg);
    bool getPositionAtTime(double time, Eigen::Vector3d& position);
    bool getVelocityAtTime(double time, Eigen::Vector3d& velocity);
    bool getYawAtTime(double time, double& yaw);
    
    double getStartTime() const { return start_time_; }
    double getEndTime() const { return end_time_; }
    bool isValid() const { return is_valid_; }

private:
    std::vector<Eigen::Vector3d> control_points_;
    std::vector<double> knots_;
    std::vector<double> yaw_points_;
    double yaw_dt_;
    double start_time_;
    double end_time_;
    int order_;
    bool is_valid_;
    
    Eigen::Vector3d evaluateBSpline(const std::vector<double>& knots, 
                                   const std::vector<Eigen::Vector3d>& control_points,
                                   int order, double t);
};

/**
 * @brief Main trajectory tracker class
 */
class TrajectoryTracker : public rclcpp_lifecycle::LifecycleNode {
public:
    explicit TrajectoryTracker(const rclcpp::NodeOptions& options = rclcpp::NodeOptions());
    
    // Lifecycle callbacks
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_configure(const rclcpp_lifecycle::State& state) override;
    
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_activate(const rclcpp_lifecycle::State& state) override;
    
    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
    on_deactivate(const rclcpp_lifecycle::State& state) override;

private:
    // Callbacks
    void trajectoryCallback(const std::shared_ptr<const traj_utils::msg::Bspline>& msg);
    void odometryCallback(const std::shared_ptr<const nav_msgs::msg::Odometry>& msg);
    void controlTimerCallback();
    
    // Control functions
    void updateControl();
    void publishVelocityCommand(const Eigen::Vector3d& linear_vel, double angular_vel_z);
    void publishVisualization();
    void emergencyStop();
    
    // State management
    void resetControllers();
    bool isTrajectoryValid() const;
    double getCurrentTime() const;
    
    // Subscribers
    rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_sub_;
    rclcpp::Subscription<traj_utils::msg::Bspline>::SharedPtr trajectory_sub_;
    
    // Publishers
    rclcpp_lifecycle::LifecyclePublisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_pub_;
    rclcpp_lifecycle::LifecyclePublisher<std_msgs::msg::Float64>::SharedPtr target_altitude_pub_;
    rclcpp_lifecycle::LifecyclePublisher<visualization_msgs::msg::Marker>::SharedPtr viz_pub_;
    
    // Timer
    rclcpp::TimerBase::SharedPtr control_timer_;
    
    // Control components
    std::unique_ptr<BSplineInterpolator> trajectory_interpolator_;
    std::unique_ptr<PIDController> pid_x_;
    std::unique_ptr<PIDController> pid_y_;
    std::unique_ptr<PIDController> pid_z_;
    std::unique_ptr<PIDController> pid_yaw_;
    
    // State variables
    Eigen::Vector3d current_position_;
    Eigen::Vector3d current_velocity_;
    Eigen::Quaterniond current_orientation_;
    double current_yaw_;
    bool has_odom_;
    bool has_trajectory_;
    
    // Parameters
    double control_frequency_;
    double max_linear_velocity_;
    double max_angular_velocity_;
    double position_tolerance_;
    double yaw_tolerance_;
    double trajectory_timeout_;
    std::string odom_frame_;
    std::string base_frame_;
    
    // Thread safety
    std::mutex state_mutex_;
    std::mutex trajectory_mutex_;
    
    // Timing
    rclcpp::Time trajectory_start_time_;
    rclcpp::Time last_trajectory_time_;
};

} // namespace ego_planner_adapter

#endif // EGO_PLANNER_ADAPTER__TRAJECTORY_TRACKER_HPP_